#include "vector3d.h"
#include "matematicas.h"

#include <QDebug>

Vector3d::Vector3d()
{
    modulo = 0;
}

Vector3d::Vector3d(const Punto3d& puntInicio, const Punto3d& puntoFin)
{
    double x = puntoFin.getX() - puntInicio.getX();
    double y = puntoFin.getY() - puntInicio.getY();
    double z = puntoFin.getZ() - puntInicio.getZ();

    setCoordenadas(x, y, z);
}

Vector3d::Vector3d(double x, double y, double z)
{
    setCoordenadas(x, y, z);
}

Vector3d::Vector3d(double modulo, const Angulo& azimut, const Angulo& colatitud)
{
    setCoordenadasEsfericas(modulo, azimut, colatitud);
}

Vector3d::~Vector3d()
{

}

bool Vector3d::operator==(const Vector3d& otroVector) const
{
    return fequals(modulo, otroVector.modulo) &&
        azimut == otroVector.azimut &&
        colatitud == otroVector.colatitud;
}

QString Vector3d::toString()
{
    return QString("Vector %1 %2 %3").arg(getModulo()).arg(getAzimut().toString()).arg(getColatitud().toString());
}

void Vector3d::sumar(const Vector3d& otroVector)
{
    Punto3d puntoExtremo = getPuntoExtremo();
    Punto3d otroPuntoExtremo = otroVector.getPuntoExtremo();
    puntoExtremo.setCoordenadas(
       puntoExtremo.getX() + otroPuntoExtremo.getX(),
       puntoExtremo.getY() + otroPuntoExtremo.getY(),
       puntoExtremo.getZ() + otroPuntoExtremo.getZ());
}

void Vector3d::desplazarPunto(Punto3d& p) const
{
    Punto3d puntoExtremo = getPuntoExtremo();
    p.setCoordenadas(
        puntoExtremo.getX() + p.getX(),
        puntoExtremo.getY() + p.getY(),
        puntoExtremo.getZ() + p.getZ());
}

double Vector3d::getModulo()
{
    return modulo;
}

void Vector3d::setModulo(double modulo)
{
    this->modulo = modulo;
}

void Vector3d::normalizar()
{
    modulo = 1;
}

Angulo Vector3d::getAzimut()
{
    return azimut;
}

Angulo Vector3d::getColatitud()
{
    return colatitud;
}

double Vector3d::calcularAzimutRadianes(double x, double y, double z)
{
    double azimutRadianes;
    if (fequals(x, 0)) {
        azimutRadianes = y < 0 ? - PI / 2 : PI / 2;
    } else if (x < 0) {
        azimutRadianes = PI + atan(y / x);
    } else if (y >= 0) {
        azimutRadianes = atan(y / x);
    } else {
        azimutRadianes = 2 * PI + atan(y / x);
    }
    return azimutRadianes;
}


double Vector3d::calcularColatitudRadianes(double x, double y, double z)
{
    double colatitudRadianes;

    if (z > 0) {
        colatitudRadianes = atan(sqrt(x * x + y * y) / z);
    } else if (z == 0) {
        colatitudRadianes = PI / 2;
    } else {
        colatitudRadianes = PI + atan(sqrt(x * x + y * y) / z);
    }
    return colatitudRadianes;
}

void Vector3d::setCoordenadas(double x, double y, double z)
{
    double azimutRadianes = calcularAzimutRadianes(x, y, z);
    azimut.setRadianes(azimutRadianes);
    
    double colatitudRadianes = calcularColatitudRadianes(x, y, z);
    colatitud.setRadianes(colatitudRadianes);
    
    modulo = sqrt(x * x + y * y + z * z);
}


void Vector3d::girar(const Angulo& giroAzimut, const Angulo& giroColatitud)
{
    azimut.sumar(giroAzimut);
    if (azimut.getGrados() <= 180) {
        colatitud.sumar(giroColatitud);
    } else {
        colatitud.restar(giroColatitud);
    }

    arreglar();

}

void Vector3d::setCoordenadasEsfericas(double modulo, const Angulo& azimut, const Angulo& colatitud)
{
    this->modulo = modulo;
    this->azimut = azimut;
    this->colatitud = colatitud;

    arreglar();
}

void Vector3d::multiplicar(double factor)
{
    modulo *= factor;
}

bool Vector3d::esNulo()
{
    return fequals(modulo, 0);
}

Punto3d Vector3d::getPuntoExtremo() const
{
    Punto3d puntoExtremo;

    double f = azimut.getRadianes();
    double q = colatitud.getRadianes();

    double sin_q = sin(q);

    double x = modulo * sin_q * cos(f);
    double y = modulo * sin_q * sin (f);
    double z = modulo * cos(q);

    puntoExtremo.setCoordenadas(x, y, z);
    return puntoExtremo;
}

/**
 * Garantiza que la colatitud este entre 0 y 180.
 */
void Vector3d::arreglar()
{
    double gradosColatiud = colatitud.getGrados();
    if (gradosColatiud > 180.0) {
        azimut.sumar(180);
        colatitud.setGrados(360 - gradosColatiud);
    }
}